#pragma once

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdarg.h>
#include <math.h>


typedef struct {
	uint8_t id;
	uint8_t params[8];
	uint8_t str;
	uint8_t numParams;
} command_t;


#define PC_MAX_PARAMS 10
#define COMMAND_ACK 1
#define COMMAND_LOGIN 255
#define COMMAND_LOGOUT 254
#define COMMAND_SET_SENSOR_INTERVAL 8
#define COMMAND_PING 4
#define COMMAND_UPDATE_DEVICE 6
#define COMMAND_UPDATE_DEVICE_DATA 5
#define COMMAND_HEARTBEAT 2
#define COMMAND_TIMEOUT 7
#define COMMAND_ECHO 0
#define COMMAND_HELLO 3

#define MIN( a, b ) (a < b) ? a : b
#define MAX( a, b ) (a > b) ? a : b

uint8_t *params[PC_MAX_PARAMS]={0};
	
command_t command;
char str[22]={'\0'};

void (*commandReceivedHandler)(command_t *cmd);

void setCommandReceptionHandler(void(*commandReceivedHandlerParam)(command_t *cmd)){
	commandReceivedHandler=commandReceivedHandlerParam;
}


void (*invalidCommandReceivedHandler)(char *text);

void setInvalidCommandReceptionHandler(void(*invalidCommandReceivedHandlerP)(char *text)){
	invalidCommandReceivedHandler=invalidCommandReceivedHandlerP;
}


/*uint8_t isValidCommandString(char * commandString){
	int length=strlen(commandString);
	return (length >=4)&&(commandString != NULL)&&(commandString[0] == '<')&&(commandString[length-2] == '\r')&&(commandString[length-1] == '\n');
}*/

void dumpCommand(command_t *command){
	writeString_P("Dumping command...\r\n");
	for(uint8_t i=0;i<PC_MAX_PARAMS;++i){
		writeString_P("Parameter ");
		writeInteger(i, DEC);
		writeChar(':');
		writeInteger(command->params[i], DEC);
		writeString_P("\r\n");
	}
	writeString_P("Done!\r\n");
}


uint8_t parseCommand(char *commandString){
	//TODO: Schnellere, neue Parsing-Routine!
	if(commandString==NULL){
		writeString_P("Not a valid command - NULL!\r\n");
		return false;
	}
	uint8_t length=strlen(commandString);
	if(length<3){
		writeString_P("Not a valid command - too short!\r\n");
		writeString(commandString);
		writeString_P("\r\n");
		return false;
	}
	if(commandString[0]!='<'){
		writeString_P("Not a valid command - does not start with <!\r\n");
		return false;
	}								
	int currentPos = 0;
	uint8_t currentParameter = 0;
	command.str=false;
	command.numParams=0;
	for(uint8_t i=0;i<8;++i){
		command.params[i]=0;
		str[i]='\0';
	}
	for(uint8_t z=8;z<21;++z){
		str[z]='\0';
	}
	command.id=atoi(&commandString[currentPos+1]);
	while (commandString[currentPos] != '\0' &&commandString[currentPos] != '\n'&& commandString[currentPos] != '$')
	{
		++currentPos;
		if(commandString[currentPos]=='i'){
			command.params[currentParameter] = atoi(&commandString[currentPos+1]); // wert auslesen
			++currentParameter;
		}else if(commandString[currentPos]=='s'){
			uint8_t slength =  atoi(&commandString[currentPos+1]); // wert auslesen
			while((commandString[currentPos] != '=')&&(commandString[currentPos] != '&') && (commandString[currentPos] != '$') && (commandString[currentPos] != '\0'&&commandString[currentPos] != '\n')){
				++currentPos;
			}
			uint8_t z;
			for(z=0;z<slength;++z){
				if(!commandString[currentPos+z+1]){
					break;
				}
				str[z]=commandString[currentPos+z+1];
			}
			str[z]='\0';
			command.str=true;
			//A string is only allowed at the last place!
			break;
		}
		while((commandString[currentPos] != '&') && (commandString[currentPos] != '$') && (commandString[currentPos] != '\0'&&commandString[currentPos] != '\n')){
			++currentPos;
		}
	}
	command.numParams=currentParameter;
	if(commandReceivedHandler){
		commandReceivedHandler(&command);
	}
	return true;
}

void sendCommand(uint8_t commandId){
	writeChar_WIFI('<');
	writeInteger_WIFI(commandId, DEC);
	writeChar_WIFI('%');
	writeChar_WIFI('$');
	writeChar_WIFI('\r');
	writeChar_WIFI('\n');
}

void sendCommand1(uint8_t commandId, uint8_t p1){
	writeChar_WIFI('<');
	writeInteger_WIFI(commandId, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p1, DEC);
	writeChar_WIFI('%');
	writeChar_WIFI('$');
	writeChar_WIFI('\r');
	writeChar_WIFI('\n');
}

void sendCommand2(uint8_t commandId, uint8_t p1, uint8_t p2){
	writeChar_WIFI('<');
	writeInteger_WIFI(commandId, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p1, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p2, DEC);
	writeChar_WIFI('%');
	writeChar_WIFI('$');
	writeChar_WIFI('\r');
	writeChar_WIFI('\n');
}

void sendCommand3(uint8_t commandId, uint8_t p1, uint8_t p2, uint8_t p3){
	writeChar_WIFI('<');
	writeInteger_WIFI(commandId, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p1, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p2, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p3, DEC);
	writeChar_WIFI('%');
	writeChar_WIFI('$');
	writeChar_WIFI('\r');
	writeChar_WIFI('\n');
}

void sendCommand4(uint8_t commandId, uint8_t p1, uint8_t p2, uint8_t p3, uint8_t p4){
	writeChar_WIFI('<');
	writeInteger_WIFI(commandId, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p1, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p2, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p3, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p4, DEC);
	writeChar_WIFI('%');
	writeChar_WIFI('$');
	writeChar_WIFI('\r');
	writeChar_WIFI('\n');
}

void sendCommand5(uint8_t commandId, uint8_t p1, uint8_t p2, uint8_t p3, uint8_t p4, uint8_t p5){
	writeChar_WIFI('<');
	writeInteger_WIFI(commandId, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p1, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p2, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p3, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p4, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p5, DEC);
	writeChar_WIFI('%');
	writeChar_WIFI('$');
	writeChar_WIFI('\r');
	writeChar_WIFI('\n');
}


void sendCommand6(uint8_t commandId, uint8_t p1, uint8_t p2, uint8_t p3, uint8_t p4, uint8_t p5, uint8_t p6){
	writeChar_WIFI('<');
	writeInteger_WIFI(commandId, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p1, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p2, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p3, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p4, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p5, DEC);
	writeChar_WIFI('&');
	writeChar_WIFI('i');
	writeInteger_WIFI(p6, DEC);
	writeChar_WIFI('%');
	writeChar_WIFI('$');
	writeChar_WIFI('\r');
	writeChar_WIFI('\n');
}